#include "gecko_bglib.h"

struct gecko_cmd_packet* gecko_wait_message(void)
{//wait for event from system
    uint32_t msg_length;
    uint32_t header;
    uint8_t  *payload;
    struct gecko_cmd_packet *pck, *retVal = NULL;
    int      ret;
    //sync to header byte
    ret = bglib_input(1, (uint8_t*)&header);
    BGLIB_DBG("ret=%#X header=%#X gecko_dev_type_gecko=%#X\r\n",ret,header,gecko_dev_type_gecko);
    if(ret < 0 || (header&0x78) != gecko_dev_type_gecko)
    {
        return 0;
    }
    ret = bglib_input(BGLIB_MSG_HEADER_LEN-1, &((uint8_t*)&header)[1]);
    BGLIB_DBG("ret=%#X header=%#X HEADER_LEN=%d\r\n",ret,header,BGLIB_MSG_HEADER_LEN);
    if(ret < 0)
    {
        return 0;
    }

    msg_length = BGLIB_MSG_LEN(header);
    BGLIB_DBG("msg_length=%d gecko_dev_type_gecko=%#X gecko_msg_type_evt=%#X\r\n",msg_length,gecko_dev_type_gecko,gecko_msg_type_evt);

    if((header & 0xf8) == (gecko_dev_type_gecko | gecko_msg_type_evt))
    {
        //received event
        BGLIB_DBG("gecko_queue_w=%d gecko_queue_r=%d BGLIB_QUEUE_LEN=%d\r\n",gecko_queue_w, gecko_queue_r, BGLIB_QUEUE_LEN);
        if ((gecko_queue_w + 1) % BGLIB_QUEUE_LEN == gecko_queue_r)
            return 0;//NO ROOM IN QUEUE

        pck=&gecko_queue[gecko_queue_w];
        gecko_queue_w = (gecko_queue_w + 1) % BGLIB_QUEUE_LEN;
        BGLIB_DBG("pck=%d gecko_queue_w=%d\r\n",pck,gecko_queue_w);
    }
    else if((header & 0xf8) == gecko_dev_type_gecko)
    {//response
        retVal = pck = gecko_rsp_msg;
        BGLIB_DBG("retVal=pck=gecko_rsp_msg=%d\r\n");
    }
    else
    {
        //fail
        return 0;
    }
    pck->header = header;
    payload = (uint8_t*)&pck->data.payload;
    BGLIB_DBG("pck->header=%d payload=%d\r\n",pck->header,payload);
    /**
    * Read the payload data if required and store it after the header.
    */
    if(msg_length)
    {
        ret = bglib_input(msg_length, payload);
        BGLIB_DBG("ret=%d\r\n",ret);
        if(ret < 0)
        {
            return 0;
        }
    }

    BGLIB_DBG("retVal=%d\r\n",retVal);
    // Using retVal avoid double handling of event msg types in outer function
    return retVal;
}


int gecko_event_pending(void)
{
    if(gecko_queue_w != gecko_queue_r)
    {//event is waiting in queue
        return 1;
    }

    //something in uart waiting to be read
	if (bglib_peek && bglib_peek())
        return 1;

    return 0;
}

struct gecko_cmd_packet* gecko_get_event(int block)
{
    struct gecko_cmd_packet* p;

    BGLIB_DBG("\r\n");
    while(1)
    {
        if(gecko_queue_w != gecko_queue_r)
        {
            p = &gecko_queue[gecko_queue_r];
            gecko_queue_r = (gecko_queue_r + 1) % BGLIB_QUEUE_LEN;
            BGLIB_DBG("\r\n");
            return p;
        }
        //if not blocking and nothing in uart -> out
        if(!block && bglib_peek && bglib_peek()==0)
        {
            BGLIB_DBG("\r\n");
            return NULL;
        }

        //read more messages from device
        if((p = gecko_wait_message()))
        {
            BGLIB_DBG("\r\n");
            return p;
        }
        if(bglib_feedog) bglib_feedog();
    }
    BGLIB_DBG("\r\n");
}

struct gecko_cmd_packet* gecko_wait_event(void)
{
    return gecko_get_event(1);
}

struct gecko_cmd_packet* gecko_peek_event(void)
{
    return gecko_get_event(0);
}

struct gecko_cmd_packet* gecko_wait_response(void)
{
    struct gecko_cmd_packet* p;
    while (1)
    {
        p = gecko_wait_message();
        if (p&&!(p->header&gecko_msg_type_evt))
            return p;
        if(bglib_feedog) bglib_feedog();
    }
}

void gecko_handle_command(uint32_t hdr, void* data)
{
    //packet in gecko_cmd_msg is waiting for output
    bglib_output(BGLIB_MSG_HEADER_LEN+BGLIB_MSG_LEN(gecko_cmd_msg->header), (uint8_t*)gecko_cmd_msg);
    gecko_wait_response();
}

void gecko_handle_command_noresponse(uint32_t hdr, void* data)
{
    //packet in gecko_cmd_msg is waiting for output
    bglib_output(BGLIB_MSG_HEADER_LEN+BGLIB_MSG_LEN(gecko_cmd_msg->header), (uint8_t*)gecko_cmd_msg);
}
